/*
 * yyt.cpp
 *
 *  Created on: 22-03-2011
 *      Author: mith
 *      version: 3.0 - for tJU
 */

#include<cstdlib>
#include<cstdio>
#include<math.h>
#include<cstring>
#include<gsl/gsl_vector.h> // GSL LIBRARY - remember use flag -lgsl -lgslcblas
#include<gsl/gsl_multiroots.h>
#include<gsl/gsl_errno.h> // to interpolation
#include<gsl/gsl_spline.h>
#include"yyt.h" // function definitions here!
using namespace std;

// DEFINITIONS
#define MAX_ITERATION 200
#define PRECISION 1e-7
#define NO_OF_EQUATIONS 5
#define DEBUG 1
#define DEEP_DEBUG 0

/*
 *********************************************************
 ************************* GSL ***************************
 *********************************************************
*/

int set (const gsl_vector * x, void *params, gsl_vector * f) {
	parameters p = *((struct parameters *) params);

	variables v;
	v.BDelta = gsl_vector_get (x, 0);
	v.chi = gsl_vector_get (x, 1);
	v.m = gsl_vector_get (x, 2);
	v.d = gsl_vector_get (x, 3);
	v.mu = gsl_vector_get (x, 4);

	Equa E(p, v);
	variables w;
	E.run(w);

	gsl_vector_set (f, 0, w.BDelta);
	gsl_vector_set (f, 1, w.chi);
	gsl_vector_set (f, 2, w.m);
	gsl_vector_set (f, 3, w.d);
	gsl_vector_set (f, 4, w.mu);

	if(DEEP_DEBUG) {
		printf("### %25.15e %25.15e %25.15e %25.15e %25.15e\n",
				gsl_vector_get (x, 0), gsl_vector_get (x, 1),
				gsl_vector_get (x, 2), gsl_vector_get (x, 3), gsl_vector_get (x, 4));
		printf("###  %24.15e# %24.15e# %24.15e# %24.15e# %24.15e#\n",
			gsl_vector_get (f, 0), gsl_vector_get (f, 1),
			gsl_vector_get (f, 2), gsl_vector_get (f, 3), gsl_vector_get (f, 4));
		printf("\n");
	}

	return GSL_SUCCESS;
}

void print_label(parameters &p) {
	printf("\nsdelta=%f, J=%f, t=%f, U=%f, L=%u\n", p.sdelta, p.J, p.t, p.U, p.L);
	printf("iter \tBDelta \t\t\tchi \t\t\tm \t\td \t\tmu\n");
}

void print_state (size_t iter, gsl_multiroot_fsolver * s)
{
	printf("%3u | %.5e (%3d) | %.5e (%3d) | %.5e (%3d) | %.5e (%3d) | %.5e (%3d)\n",
			iter, gsl_vector_get (s->x, 0), (int)(log(abs(gsl_vector_get (s->f, 0)))/log(10)),
			gsl_vector_get (s->x, 1), (int)(log(abs(gsl_vector_get (s->f, 1)))/log(10)),
			gsl_vector_get (s->x, 2), (int)(log(abs(gsl_vector_get (s->f, 2)))/log(10)),
			gsl_vector_get (s->x, 3), (int)(log(abs(gsl_vector_get (s->f, 3)))/log(10)),
			gsl_vector_get (s->x, 4), (int)(log(abs(gsl_vector_get (s->f, 4)))/log(10)));
}

void print(FILE * stream, double x, variables &v) {
	fprintf(stream, "%f %.15e %.15e %.15e %.15e %.15e\n", (float)x, v.BDelta, v.chi, v.m, v.d, v.mu);
	fflush (stream);
}

int aPoint(parameters &p, variables &v) {
	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;
	int status;
	size_t iter = 0;
	const size_t n = NO_OF_EQUATIONS;
	gsl_multiroot_function f = {&set, n, &p};

	double x_init[n];
	x_init[0] = v.BDelta;
	x_init[1] = v.chi;
	x_init[2] = v.m;
	x_init[3] = v.d;
	x_init[4] = v.mu;

	gsl_vector *x = gsl_vector_alloc (n);
	for(size_t i=0; i<n; i++) { gsl_vector_set (x, i, x_init[i]); }

	T = gsl_multiroot_fsolver_hybrids;
	s = gsl_multiroot_fsolver_alloc (T, n);
	gsl_multiroot_fsolver_set (s, &f, x);

	if(DEBUG) {
		print_label(p);
		print_state (iter, s);
	}

	do {
		iter++;
		status = gsl_multiroot_fsolver_iterate (s);
		if(DEBUG) print_state (iter, s);
	    if(status) break;	// check if solver is stuck
	    status = gsl_multiroot_test_residual (s->f, PRECISION);
	}
	while (status == GSL_CONTINUE && iter < MAX_ITERATION);

	v.BDelta = gsl_vector_get (s->x, 0);
	v.chi = gsl_vector_get (s->x, 1);
	v.m = gsl_vector_get (s->x, 2);
	v.d = gsl_vector_get (s->x, 3);
	v.mu = gsl_vector_get (s->x, 4);

	printf ("status = %s\n", gsl_strerror(status));
	gsl_multiroot_fsolver_free (s);
	gsl_vector_free (x);
	return status;
}

/*
 *********************************************************
 ******************** INTERPOLATION **********************
 *********************************************************
*/

//double extrapolation(double &x, double &y, double point) {
//	gsl_interp_accel *acc = gsl_interp_accel_alloc ();
//	gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, POINTS_TO_INTERP);
//
//	gsl_spline_init (spline, x, y, POINTS_TO_INTERP);
//	double w = gsl_spline_eval (spline, point, acc);
//
//	gsl_spline_free (spline);
//	gsl_interp_accel_free (acc);
//	return w;
//}

/*
 *********************************************************
 ************************ MAIN ***************************
 *********************************************************
*/

#define POINTS_TO_INTERP 5
#define RESOLUTION 100.
#define START 0.01
#define STOP 0.35
#define OUTPUT_FILE "../mros3_plots/test/test.dat"

int yyt (void) {
	struct parameters p;
	p.sdelta = 0.1;
	p.J = 1./3.;
	p.t = 1.;
	p.U = 20;
	p.L = 512;

	struct variables v;
	v.BDelta = 7.471152592513997e-02;//5.107639205382098e-03;
	v.chi = 1.723602618662765e-01;
	v.m = 2.151449386164908e-01; //2.145465678141835e-1;
	v.d = 8.046487812950842e-05;
	v.mu = -1.808986198538001e-01;

	// 0.010000 7.471152592513997e-02 1.723602618662765e-01 2.151449386164908e-01 8.046487812950842e-05 -1.808986198538001e-01

	FILE * pFile;
	pFile = fopen(OUTPUT_FILE, "w");
	fprintf(pFile, "sdelta=%f, J=%f, t=%f, U=%f, L=%u\n", p.sdelta, p.J, p.t, p.U, p.L);
	fprintf(pFile, "zmiana sdelta od 0 do 0.35\n");
	fprintf(pFile, "# x \t\t\tBDelta \t\t\tchi \t\t\tm \t\t\td \t\t\tmu\n");

	double step = ((double)(STOP - START))/((double)RESOLUTION);
	double epsilon = abs(step)/10.;

	p.sdelta = START;
	variables old;
	aPoint(p, v);
	addTo(v, old);

	for(double i=START; i<STOP+epsilon; i+=step) {
		p.sdelta = i;

		int status = aPoint(p, v); // aPoint take initiate point from v vector and put results back to v
		if(status != GSL_SUCCESS) {
			continue;
		}
		print(pFile, i, v);

		variables temp;
		addTo(v, temp);
		v.BDelta += v.BDelta - old.BDelta;
		v.chi += v.chi - old.chi;
		v.m += v.m - old.m;
		v.d += v.d - old.d;
		v.mu += v.mu - old.mu;
		addTo(temp, old);

		// aPoint powinno zwracać status. Jeśli nie tak to np. cofnąć się? (zagęścić pomiary)
		// dodać opcję ekstrapolacji
	}

	fclose (pFile);
	return 0;
}
